﻿// Inner Fire 游戏引擎库
// JointConstraint - 关节约束
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 用于关节约束。头文件在constraint.h。此CPP用于分装，没有头文件
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-06-30

#include "constraint.h"
#include <game/physics/rigid_body.h>


namespace ifire::game {

JointConstraint::JointConstraint() : Constraint() {}

JointConstraint::JointConstraint(RigidBody* a, RigidBody* b, IVec3 anchorPoint)
    : Constraint() {
  this->a = a;
  this->b = b;
  this->aPoint = a->WorldToLocal(anchorPoint);
  this->bPoint = b->WorldToLocal(anchorPoint);
}

void JointConstraint::PreSolve(float dt) {
  // 获取世界空间中的锚点位置
  auto local_a = a->LocalToWorld(aPoint);
  auto local_b = b->LocalToWorld(bPoint);
  const IVec pa = XMLoadFloat3(&local_a);
  const IVec pb = XMLoadFloat3(&local_b);

  auto a_pos = XMLoadFloat3(&a->position_);
  auto b_pos = XMLoadFloat3(&b->position_);
  const IVec ra = pa - a_pos;
  const IVec rb = pb - b_pos;

  // 3D Jacobian矩阵大小为1x12 (1个约束 × 12自由度)
  jacobian.Zero();

  // 计算Jacobian分量
  IVec J1 = (pa - pb);
  jacobian[0][0] = XMVectorGetX(J1); // A linear x
  jacobian[0][1] = XMVectorGetY(J1); // A linear y
  jacobian[0][2] = XMVectorGetZ(J1); // A linear z

  IVec J2 = XMVector3Cross(ra, (pa - pb));
  jacobian[0][3] = XMVectorGetX(J2); // A angular x
  jacobian[0][4] = XMVectorGetY(J2); // A angular y
  jacobian[0][5] = XMVectorGetZ(J2); // A angular z

  IVec J3 = (pb - pa);
  jacobian[0][6] = XMVectorGetX(J3); // B linear x
  jacobian[0][7] = XMVectorGetY(J3); // B linear y
  jacobian[0][8] = XMVectorGetZ(J3); // B linear z

  IVec J4 = XMVector3Cross(rb, (pb - pa));
  jacobian[0][9] = XMVectorGetX(J4);  // B angular x
  jacobian[0][10] = XMVectorGetY(J4); // B angular y
  jacobian[0][11] = XMVectorGetZ(J4); // B angular z

  // 预热启动(应用缓存的lambda)
  const MatMN Jt = jacobian.Transpose();

  // 修改预热冲量应用方式
  if (cached_lambda[0] > FLT_EPSILON) {
    auto impulses = Jt * cached_lambda * 0.8f; // 阻尼系数
    // 对A和B施加冲量
    a->ApplyImpulseLinear(IVec3(impulses[0], impulses[1], impulses[2]));
    a->ApplyImpulseAngular(IVec3(impulses[3], impulses[4], impulses[5]));
    b->ApplyImpulseLinear(IVec3(impulses[6], impulses[7], impulses[8]));
    b->ApplyImpulseAngular(IVec3(impulses[9], impulses[10], impulses[11]));
  }

  // 计算baugarte稳定项
  auto delta = pb - pa;
  // 更稳定的Baumgarte项
  const float beta = 0.1f;   // 降低刚度系数
  const float slop = 0.001f; // 允许的穿透容差
  float C = XMVectorGetX(XMVector3LengthSq(delta));
  bias = (beta / dt) * (std::max)(0.0f, C - slop);
}

void JointConstraint::Solve() {

  const auto V = GetVelocities();
  const auto invM = GetInvM();

  const auto& J = jacobian;
  const auto Jt = jacobian.Transpose();

  // Calculate the numerator
  auto lhs = J * invM * Jt; // A
  auto rhs = J * V * -1.0f; // b
  rhs[0] -= bias;

  // Solve the values of lambda using Ax=b (Gaus-Seidel method)
  auto lambda = MatMN<1, 1>::SolveGaussSeidel(lhs, rhs);
  lambda[0] = std::clamp(lambda[0], -1.0f, 1.0f);
  cached_lambda += lambda * 0.8f; // 阻尼累积冲量

  // Apply the impulses to both A and B
  auto impulses = Jt * lambda;
  a->ApplyImpulseLinear(IVec3(impulses[0], impulses[1], impulses[2]));
  a->ApplyImpulseAngular(IVec3(impulses[3], impulses[4], impulses[5]));
  b->ApplyImpulseLinear(IVec3(impulses[6], impulses[7], impulses[8]));
  b->ApplyImpulseAngular(IVec3(impulses[9], impulses[10], impulses[11]));
}

void JointConstraint::PostSolve() {}

} // namespace ifire::game